/**
 * @file map.cpp
 * @author circleup (circleup@foxmial.com)
 * @brief 地图加载节点（规划路径及通过激光雷达生成的虚拟路径）
 * @version 0.1
 * @date 2020-07-14
 * 
 * @copyright Copyright (c) 2020
 * 
 */

#include "global.h"

#include "screen_driver.h"

#include "map.h"
#include "map_driver.h"

#include <tf/transform_listener.h>
#include <tf/tf.h>

#include "ros/ros.h"

int main(int argc, char **argv)
{
  ros::init(argc, argv, "MAP");
  ros::NodeHandle n;
  ros::Rate loop_rate(10);

  ROS_INFO("This is MAP.");

  class map map;
  class path path;

  tf::TransformListener listener;
  geometry_msgs::Vector3Stamped pose;
  
  if (map.AddPlanPoint())
  {
    ROS_INFO(" Loaded piont size is %d", map.g_points_plan.size());
  }
  else
  {
    ROS_INFO(" Open GPS File Failed!!");
  }

  while (ros::ok())
  {
    tf::StampedTransform transform;

    geometry_msgs::PointStamped path_pose;
    path_pose.header.frame_id = "/car";

    for (uint16_t i = 0; i < map.g_points_plan.size(); i++)
    {
      path_pose.header.stamp = ros::Time(0);
      path_pose.point.x = map.g_points_plan[i].E;
      path_pose.point.y = map.g_points_plan[i].N;

      try{
        listener.waitForTransform("/plan", "/car", ros::Time(0), ros::Duration(1.0));

        geometry_msgs::PointStamped base_point;
        listener.transformPoint("/plan", path_pose, base_point);
        path.this_pose.header.stamp = ros::Time::now();
        path.this_pose.header.frame_id = "/car";
        path.this_pose.pose.position.x = base_point.point.x;
        path.this_pose.pose.position.y = base_point.point.y;
        path.add_pose(path.this_pose);
      }
      catch(tf::TransformException& ex){
        ROS_ERROR("%s", ex.what());
      }
    }
    
    path.publish_to_rviz();
    path.path_rviz.poses.clear();
    loop_rate.sleep();
    ros::spinOnce();
  }

  return 0;
}